#include "bsp_motor.h"
#include "bsp_psw.h"
#include "bsp_distance.h"
#include "bsp_servo.h"
#include "status.h"
#include "handle.h"

#define BUZZER 22

int input_int = 0;
int team = NO_TEAM;
int backTime = 0;
int stopTime = 0;

int getSerial_int(void);
void buzzer(char mode);
void choiceTeam(void);

void setup()
{
  
  pinMode(BUZZER,OUTPUT);             //蜂鸣器初始化
  digitalWrite(BUZZER,LOW);
  
  Serial.begin(38400);                //电脑串口通信
  Serial1.begin(9600);                //摄像头串口通信
  Serial2.begin(9600);                //蓝牙串口通信
  Motor_Init();                       
  psw_Init();
  distance_Init();
  Servo_Init();
  delay(1000);

//  while(!(Serial1.read() == 's'));
//  buzzer('s');

  //选择队伍
  while(team == NO_TEAM)
  {
    choiceTeam();
  }
  Serial2.println(team);
  getUpStage();
  delay(200);
}

void loop()
{ 
   if(OU_verdict())
   {
    ServoRun(LIFT);
    while(flag_backStage == 0)
    {
      fallBack();
      Serial2.println("down");
    }
    flag_backStage = 0;
   }
   else
   {
    Serial2.println("up");
    ServoRun(DOWN);
    isOnStage();
   while(flag_OnStage)       //在擂台上
   {
    
     Forward(MIDSPEED);
     isObject();             //判断是否有物体
     isOnStage();            //判断是否在台上

     while(flag_object && flag_OnStage)         //有物体
     {
       Serial2.print("obj");
       objAttack();
     }
                
   }

   backTime = millis();
   stopTime = 0;
   while (!flag_OnStage && stopTime < 800)
   {  
     Serial2.println("out");
     outBack();
     stopTime = millis() - backTime;
     Serial2.println(stopTime);
   }
   brack();
  }
}


/**
   @brief 得到串口监视器的输入值
   @param void
   @return int
 * */
int getSerial_int()
{

  if (Serial.available())
  {
    input_int = Serial.parseInt();
  }
  return input_int;
}
/**
   @brief 控制蜂鸣器发出声音
   @param void
   @return int mode s:摄像头运行成功 RED_TEAM:选择红队 BLUE_TEAM:选择蓝队 NO_TEAM:取消选择队伍
 * */
void buzzer(char mode)
{
  switch (mode)
  {
  case 's':
    digitalWrite(BUZZER,HIGH);      //一短
    delay(200);
    digitalWrite(BUZZER,LOW);
      break;
  case RED_TEAM:
    digitalWrite(BUZZER,HIGH);      //两长
    delay(500);
    digitalWrite(BUZZER,LOW);
    delay(100);
    digitalWrite(BUZZER,HIGH);
    delay(500);
    digitalWrite(BUZZER,LOW);
    break;
  case NO_TEAM:
    digitalWrite(BUZZER,HIGH);      //两短一长
    delay(200);
    digitalWrite(BUZZER,LOW);
    delay(100);
    digitalWrite(BUZZER,HIGH);
    delay(200);
    digitalWrite(BUZZER,LOW);
    delay(100);
    digitalWrite(BUZZER,HIGH);      
    delay(500);
    digitalWrite(BUZZER,LOW);
    break;
  case BLUE_TEAM:
    digitalWrite(BUZZER,HIGH);      //三短
    delay(200);
    digitalWrite(BUZZER,LOW);
    delay(50);
    digitalWrite(BUZZER,HIGH);      
    delay(200);
    digitalWrite(BUZZER,LOW);
    delay(50);
    digitalWrite(BUZZER,HIGH);      
    delay(200);
    digitalWrite(BUZZER,LOW);
    break;
   default:
    break;
  }
}
void choiceTeam()
{
  unsigned long tempTime = 0;
  while(team == NO_TEAM)
  {
    if (!digitalRead(DOWN_L_PSW)) team = RED_TEAM;
    else if(!digitalRead(DOWN_R_PSW)) team = BLUE_TEAM;         
  }
  buzzer(team);
  
//  tempTime = millis();
//  while(millis() - tempTime <3000);
//  {
//    
//    if(!digitalRead(UP_LF_PSW))
//    {
//      team = NO_TEAM;
//      buzzer(team);
//      return;
//    }
//  }
}
